# 将RTK和识别位置画图分析

import matplotlib.pyplot as plt
import pandas as pd
import math

RTK_CSV_NAME = 'data_rtk.csv'
DETECT_CSV_NMAE = 'data_detect.csv'

rtk_data_csv = pd.read_csv(RTK_CSV_NAME)  # 读取刚才写入的文件
detect_data_csv = pd.read_csv(DETECT_CSV_NMAE)  # 读取刚才写入的文件

RTKtime = rtk_data_csv["RTKtime"]
rx = rtk_data_csv["rx"]
ry = rtk_data_csv["ry"]
rz = rtk_data_csv["rz"]
rr = rtk_data_csv["rr"]
rdis = rtk_data_csv["distance"]

detectTime = detect_data_csv["detectTime"]
x = detect_data_csv["x"]
y = detect_data_csv["y"]
z = detect_data_csv["z"]
r = detect_data_csv["r"]
dis = detect_data_csv["dis"]

last_time = detectTime[0]
i = 1
for time_ in detectTime:
    if abs(time_ - last_time) > 30:
        i += 1
    last_time = time_

time_lists = [[] for _ in range(i)]
x_lists = [[] for _ in range(i)]
y_lists = [[] for _ in range(i)]
z_lists = [[] for _ in range(i)]
r_lists = [[] for _ in range(i)]
d_lists = [[] for _ in range(i)]

rtk_time_lists = [[] for _ in range(i)]
rtk_x_lists = [[] for _ in range(i)]
rtk_y_lists = [[] for _ in range(i)]
rtk_z_lists = [[] for _ in range(i)]
rtk_r_lists = [[] for _ in range(i)]
rtk_d_lists = [[] for _ in range(i)]

last_time = detectTime[0]
i = 0
for x_, y_, z_, r_, time_, dis_ in zip(x, y, z, r, detectTime, dis):
    if abs(time_ - last_time) > 30:
        i += 1
    last_time = time_
    time_lists[i].append(time_)
    x_lists[i].append(x_)
    y_lists[i].append(y_)
    z_lists[i].append(z_)
    r_lists[i].append(r_)
    d_lists[i].append(dis_)

last_time = RTKtime[0]
i = 0
record_flag = False
add_flag = False
for rx_, ry_, rz_, rr_, time_, rd_ in zip(rx, ry, rz, rr, RTKtime, rdis):
    if time_lists[i][0] <= time_ and time_ <= time_lists[i][-1]:
        record_flag = True
        add_flag = False
    elif time_ > time_lists[i][-1]:
        if add_flag == False and i < (len(time_lists) - 1):
            i += 1
            add_flag = True
        record_flag = False

    if record_flag == True:
        rtk_time_lists[i].append(time_)
        rtk_x_lists[i].append(rx_)
        rtk_y_lists[i].append(ry_)
        rtk_z_lists[i].append(rz_)
        rtk_r_lists[i].append(rr_)
        rtk_d_lists[i].append(rd_)

# 1
# fig_one = plt.figure(num=1, figsize=(8, 8))
# ax1_x = fig_one.add_subplot(221)
# ax1_x.plot(time_lists[0], x_lists[0], 'r.')
# ax1_x.plot(rtk_time_lists[0], rtk_x_lists[0], 'b.')
# ax1_x.set_title("x1", fontsize=24)

# ax1_y = fig_one.add_subplot(222)
# ax1_y.plot(time_lists[0], y_lists[0], 'r.')
# ax1_y.plot(rtk_time_lists[0], rtk_y_lists[0], 'b.')
# ax1_y.set_title("y1", fontsize=24)

# ax1_d = fig_one.add_subplot(211)
# ax1_d.plot(time_lists[0], d_lists[0], 'r.')
# ax1_d.plot(rtk_time_lists[0], rtk_d_lists[0], 'b.')
# ax1_d.set_title("distance", fontsize=24)

# ax1_z = fig_one.add_subplot(223)
# ax1_z.plot(time_lists[0], z_lists[0], 'r.')
# ax1_z.plot(rtk_time_lists[0], rtk_z_lists[0], 'b.')
# ax1_z.set_title("z1", fontsize=24)

# ax1_r = fig_one.add_subplot(224)
# ax1_r.plot(time_lists[0], r_lists[0], 'r.')
# ax1_r.plot(rtk_time_lists[0], rtk_r_lists[0], 'b.')
# ax1_r.set_title("r1", fontsize=24)

# # 2
# fig_two = plt.figure(num=2, figsize=(8, 8))
# ax2_x = fig_two.add_subplot(221)
# ax2_x.plot(time_lists[1], x_lists[1], 'r.')
# ax2_x.plot(rtk_time_lists[1], rtk_x_lists[1], 'b.')
# ax2_x.set_title("x2", fontsize=24)

# ax2_y = fig_two.add_subplot(222)
# ax2_y.plot(time_lists[1], y_lists[1], 'r.')
# ax2_y.plot(rtk_time_lists[1], rtk_y_lists[1], 'b.')
# ax2_y.set_title("y2", fontsize=24)

# ax2_d = fig_two.add_subplot(211)
# ax2_d.plot(time_lists[1], d_lists[1], 'r.')
# ax2_d.plot(rtk_time_lists[1], rtk_d_lists[1], 'b.')
# ax2_d.set_title("distance", fontsize=24)

# ax2_z = fig_two.add_subplot(223)
# ax2_z.plot(time_lists[1], z_lists[1], 'r.')
# ax2_z.plot(rtk_time_lists[1], rtk_z_lists[1], 'b.')
# ax2_z.set_title("z2", fontsize=24)

# ax2_r = fig_two.add_subplot(224)
# ax2_r.plot(time_lists[1], r_lists[1], 'r.')
# ax2_r.plot(rtk_time_lists[1], rtk_r_lists[1], 'b.')
# ax2_r.set_title("r2", fontsize=24)

# # 3
# fig_three = plt.figure(num=3, figsize=(8, 8))
# ax3_x = fig_three.add_subplot(221)
# ax3_x.plot(time_lists[2], x_lists[2], 'r.')
# ax3_x.plot(rtk_time_lists[2], rtk_x_lists[2], 'b.')
# ax3_x.set_title("x3", fontsize=24)

# ax3_y = fig_three.add_subplot(222)
# ax3_y.plot(time_lists[2], y_lists[2], 'r.')
# ax3_y.plot(rtk_time_lists[2], rtk_y_lists[2], 'b.')
# ax3_y.set_title("y3", fontsize=24)

# ax3_d = fig_three.add_subplot(211)
# ax3_d.plot(time_lists[2], d_lists[2], 'r.')
# ax3_d.plot(rtk_time_lists[2], rtk_d_lists[2], 'b.')
# ax3_d.set_title("distance", fontsize=24)

# ax3_z = fig_three.add_subplot(223)
# ax3_z.plot(time_lists[2], z_lists[2], 'r.')
# ax3_z.plot(rtk_time_lists[2], rtk_z_lists[2], 'b.')
# ax3_z.set_title("z3", fontsize=24)

# ax3_r = fig_three.add_subplot(224)
# ax3_r.plot(time_lists[2], r_lists[2], 'r.')
# ax3_r.plot(rtk_time_lists[2], rtk_r_lists[2], 'b.')
# ax3_r.set_title("r3", fontsize=24)

# 4
fig_four = plt.figure(num=4, figsize=(8, 8))
# ax4_x = fig_four.add_subplot(221)
# ax4_x.plot(time_lists[3], x_lists[3], 'r.')
# ax4_x.plot(rtk_time_lists[3], rtk_x_lists[3], 'b.')
# ax4_x.set_title("x4", fontsize=24)

# ax4_y = fig_four.add_subplot(222)
# ax4_y.plot(time_lists[3], y_lists[3], 'r.')
# ax4_y.plot(rtk_time_lists[3], rtk_y_lists[3], 'b.')
# ax4_y.set_title("y4", fontsize=24)

ax4_d = fig_four.add_subplot(311)
ax4_d.plot(time_lists[3], d_lists[3], 'r.')
ax4_d.plot(rtk_time_lists[3], rtk_d_lists[3], 'b.')
ax4_d.set_title("xy_distance", fontsize=24)

ax4_z = fig_four.add_subplot(312)
ax4_z.plot(time_lists[3], z_lists[3], 'r.')
ax4_z.plot(rtk_time_lists[3], rtk_z_lists[3], 'b.')
ax4_z.set_title("z", fontsize=24)

ax4_r = fig_four.add_subplot(313)
ax4_r.plot(time_lists[3], r_lists[3], 'r.')
ax4_r.plot(rtk_time_lists[3], rtk_r_lists[3], 'b.')
ax4_r.set_title("r", fontsize=24)

plt.show()

# fig_one = plt.figure(num=1, figsize=(4, 4))
# ax1_x = fig_one.add_subplot(111)
# ax1_x.plot(rtk_x_lists[0], rtk_y_lists[0], "b")
# ax1_x.set_title("rtk", fontsize=24)

# fig_two = plt.figure(num=2, figsize=(4, 4))
# ax2_x = fig_two.add_subplot(111)
# ax2_x.plot(x_lists[0], y_lists[0], "b")
# ax2_x.set_title("detect", fontsize=24)

# plt.show()

# plt.plot(rtk_x_lists[0], rtk_y_lists[0], "b")
# #设置图标标题
# plt.title("Square Numbers", fontsize=24)
# #设置坐标轴标签
# plt.xlabel("Value", fontsize=14)
# plt.ylabel("Square of Value", fontsize=14)
# #设置刻度标记的大小
# plt.tick_params(axis='both', labelsize=14)
# #打开matplotlib查看器，并显示绘制图形
# plt.show()
